﻿/*
 * @author: wizardc
 */

using System.Collections.Generic;

namespace Dou.Pathfinding
{
    /// <summary>
    /// 基于图的寻路
    /// - 2 个节点之间的消耗 cost 不能为负数；
    /// - 2 个节点之间的连接是单向的，如果需要双向连接请调用 2 次设置连接方法 `SetNodeLink(a, b, 1);SetNodeLink(b, a, 1);`；
    /// </summary>
    public class Dijkstra
    {
        protected DijkstraMap _map;

        private DijkstraNode _startNode;
        private DijkstraNode _endNode;

        private List<string> _path;

        public List<string> Path => _path;

        public float FindPath(DijkstraMap grid)
        {
            if (_map != null)
            {
                Clear();
            }

            _map = grid;
            _startNode = _map.StartNode;
            _endNode = _map.EndNode;
            return Search();
        }

        protected float Search()
        {
            var start = _startNode;
            var end = _endNode;
            _path = new List<string>();
            if (start == end)
            {
                return 0;
            }

            var queue = new PriorityQueue();
            var distances = new Dictionary<string, float>();
            var previous = new Dictionary<string, string>();

            foreach (var node in _map.List)
            {
                if (node == start)
                {
                    distances.Add(node.name, 0);
                    queue.Enqueue(node.name, 0);
                } else
                {
                    distances.Add(node.name, float.MaxValue);
                    queue.Enqueue(node.name, float.MaxValue);
                }
            }

            while (!queue.Empty)
            {
                var smallest = queue.Dequeue();
                if (smallest == end.name)
                {
                    while (previous.ContainsKey(smallest))
                    {
                        _path.Add(smallest);
                        smallest = previous[smallest];
                    }

                    break;
                }

                if (string.IsNullOrEmpty(smallest) || float.IsInfinity(distances[smallest]))
                {
                    continue;
                }

                var link = _map.GetNode(smallest).link;
                foreach (var kv in link)
                {
                    var neighbor = kv.Key;
                    var alt = distances[smallest] + link[neighbor].cost;
                    if (alt < distances[neighbor])
                    {
                        distances[neighbor] = alt;
                        previous[neighbor] = smallest;
                        queue.Enqueue(neighbor, alt);
                    }
                }
            }

            if (_path.Count == 0)
            {
                return -1;
            }

            _path.Add(start.name);
            _path.Reverse();
            float cost = 0;
            for (int i = 0, len = _path.Count - 1; i < len; i++)
            {
                var startName = _path[i];
                var endName = _path[i + 1];
                cost += _map.GetNode(startName).link[endName].cost;
            }

            return cost;
        }

        public void Clear()
        {
            _map = null;
            _startNode = null;
            _endNode = null;
            _path = null;
        }
    }

    class PriorityQueue
    {
        private List<PriorityNode> _nodes = new List<PriorityNode>();

        public bool Empty => _nodes.Count == 0;

        public void Enqueue(string name, float cost)
        {
            _nodes.Add(new PriorityNode
            {
                name = name,
                cost = cost
            });
            _nodes.Sort(SortFunc);
        }

        private int SortFunc(PriorityNode a, PriorityNode b)
        {
            if (a.cost == b.cost)
            {
                return 0;
            }

            return a.cost > b.cost ? 1 : -1;
        }

        public string Dequeue()
        {
            var name = _nodes[0].name;
            _nodes.RemoveAt(0);
            return name;
        }
    }

    class PriorityNode
    {
        public string name;
        public float cost;
    }
}
